#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/State.h>

#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandBool.h>//用来进行无人机解锁,发送True则为解锁，返回success值为True表示上电/断电操作执行成功，返回值result(int32)，如果解锁/上锁操作成功执行，result值为0
#include <geometry_msgs/PoseStamped.h>//用来进行发送目标位置,带有头消息和位姿的消息
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Float64.h>//可用来存储Double类型的数据
#include <std_msgs/Int8.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/ParamSet.h>
#include <flydatas_msgs/Flydatas.h>
#include <csignal> // 引入 signal 和 SIGINT 的定义
#include <cstring> // for memcpy
#include <thread>  // 包含 thread 库，用于休眠
#include <chrono>  // 包含 chrono 库
#include <mavros_msgs/CommandHome.h>    //用于设置GPS原点

//用于提取文件名，打印日志
#include <boost/filesystem.hpp>
#include <iostream>
// 使用 __FILE__ 宏获取当前文件的路径
std::string file_path = __FILE__;
// 使用 boost::filesystem 提取文件名
std::string file_name = boost::filesystem::path(file_path).filename().string();

ros::ServiceClient set_mode_client;  // 声明为全局变量

void signalHandler(int signum) {//Ctrl+C退出时调用此函数，切换无人机的模式为自动定点模式
    ROS_INFO("Shutting down... Switching to LOITER mode.");
    mavros_msgs::SetMode hold_mode;
    hold_mode.request.custom_mode = "AUTO.LOITER";  // 切换到 LOITER 模式
    for(int i=0;i<10;i++){//尝试10次切到定点模式
        if (set_mode_client.call(hold_mode) && hold_mode.response.mode_sent) {
            ROS_INFO("Switched to LOITER mode. The drone will maintain its position.");
            break;
        }
    }
    ros::shutdown(); // 结束 ROS
    // 退出程序
    exit(signum);
}

mavros_msgs::State current_state;
geometry_msgs::PoseStamped local_pos;//用于存储订阅到的无人机的姿态信息（位置等）
geometry_msgs::PoseStamped last_local_pos;//上次订阅到的无人机的姿态信息（位置等）
sensor_msgs::NavSatFix global_pos;//订阅gps坐标等信息
flydatas_msgs::Flydatas flydatas;//订阅控制消息
flydatas_msgs::Flydatas pub_flydatas;//发布控制消息


void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
    pub_flydatas.flight_state = *msg;
}

void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
    last_local_pos = local_pos; // 保存上次的姿态信息
    local_pos = *msg;           // 更新当前的姿态信息
    pub_flydatas.pos = *msg;
}

void global_pos_Callback(const sensor_msgs::NavSatFix::ConstPtr& msg) {
    global_pos = *msg;
    pub_flydatas.gps[0].latitude = global_pos.latitude;//刷新发给前端的gps数据
    pub_flydatas.gps[0].longitude = global_pos.longitude;
    pub_flydatas.gps[0].altitude = global_pos.altitude;
}

void flydatas_Callback(const flydatas_msgs::Flydatas::ConstPtr& msg){
    flydatas = *msg;
    ROS_INFO("flydatas_Callback is do");
}


// WGS84常量
const double R_EARTH = 6371000; // 地球半径，单位为米
// 将经纬度转换为END（东-北-上）坐标
//当经纬度均相差0.000001时，大致相差10cm左右，例：21.679736, 110.918791, 21.679735, 110.918790    dx:-0.103329,dy:-0.111195
void globalToLocalEND(double lat1, double lon1, double lat2, double lon2, double& dx, double& dy) {
    double dLat = (lat2 - lat1) * M_PI / 180.0;
    double dLon = (lon2 - lon1) * M_PI / 180.0;
    
    // 计算纬度平均值的余弦
    double latAvg = (lat1 + lat2) / 2.0 * M_PI / 180.0;
    double cosLat = cos(latAvg);
    
    // 计算东（dx）和北（dy）的距离，单位为米
    dx = R_EARTH * dLon * cosLat;
    dy = R_EARTH * dLat;
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "fly_control_node");
    ros::NodeHandle nh;//实例化ROS句柄，这个ros::NodeHandle类封装了ROS中的一些常用功能

    // 注册 SIGINT 信号处理程序
    signal(SIGINT, signalHandler);

    //订阅者节点
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
    ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 10, local_pos_callback);
    ros::Subscriber global_pos_sub = nh.subscribe<sensor_msgs::NavSatFix>("/mavros/global_position/global", 10, global_pos_Callback);
    ros::Subscriber flydatas_sub = nh.subscribe<flydatas_msgs::Flydatas>("/pub_flydatas", 10, flydatas_Callback);//订阅控制消息


    //发布者节点
    ros::Publisher velocity_pub = nh.advertise<geometry_msgs::TwistStamped>("mavros/setpoint_velocity/cmd_vel", 10);
    ros::Publisher local_pos_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);//这是一个发布者对象，用来在本地坐标系下发布目标点，后面会以20Hz频率发布目标点
    ros::Publisher flydatas_pub = nh.advertise<flydatas_msgs::Flydatas>("/sub_flydatas", 10);//发布控制消息给其他地方订阅
    
    //服务节点
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");//一个客户端，用来解锁无人机，这是因为无人机如果降落后一段时间没有收到信号输入，会自动上锁来保障安全
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");//一个客户端，用来切换飞行模式
    ros::ServiceClient param_set_client = nh.serviceClient<mavros_msgs::ParamSet>("/mavros/param/set");
    ros::Rate rate10(10.0);  // 任务时10Hz,rate10表示10Hz
    ros::Rate rate5(5.0);  // 非任务时5Hz,rate5标志5Hz

    //// 等待飞控和MAVROS建立连接，current_state是我订阅的MAVROS的状态，在收到心跳包之后连接成功跳出循环
    while (ros::ok() && !current_state.connected) {
        ros::spinOnce();
        rate10.sleep();
    }

    //定义一个变量来存储要设置的飞行模式
    /*
        可以设置的飞行模式为：
        MANUAL  手动
        ACRO    特技
        ALTCTL  高度
        POSCTL  定点
        OFFBOARD    外部模式
        STABILIZED  自稳
        RATTITUDE
        AUTO.LOITER     保持当前位置模式
        AUTO.RTL        自动返航模式
        AUTO.TAKEOFF    自动起飞模式
        AUTO.LAND       自动降落模式
        AUTO.MISSION    执行预设航点任务模式
        AUTO.RTGS       自动返回到航点（Return to GPS）
        AUTO.READY      准备模式。此模式表示无人机已准备好执行任务，但尚未启动。
    */
    mavros_msgs::State my_px4_mode;
    my_px4_mode.mode = "OFFBOARD";        //默认模式为“ POSCTL ”（定点模式）
    //建立一个类型为SetMode的服务端offb_set_mode，并将其中的模式mode设为"my_px4_mode.mode"，作用便是用于后面的客户端与服务端之间的通信（服务）
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = my_px4_mode.mode;
    ROS_INFO("%s:want my_px4_mode is %s", file_name.c_str(),my_px4_mode.mode.c_str());
    /*
    AUTO.LOITER     保持当前位置模式
    AUTO.RTL        自动返航模式
    AUTO.TAKEOFF    自动起飞模式
    AUTO.LAND       自动降落模式
    AUTO.MISSION    执行预设航点任务模式
    AUTO.RTGS       自动返回到航点（Return to GPS）
    AUTO.READY      准备模式。此模式表示无人机已准备好执行任务，但尚未启动。
    */
    mavros_msgs::SetMode hold_mode;//定义一个变量来设置无人机的自动模式
    hold_mode.request.custom_mode = "AUTO.LOITER";//默认AUTO.LOITER:保持当前位置模式

    //建立一个类型为CommandBool的服务端arm_cmd，并将其中的是否解锁设为"true"，作用便是用于后面的客户端与服务端之间的通信（服务）
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    ros::Time last_request_time = ros::Time::now();//更新判断模式时间
    ros::Time last_local_pos_time = ros::Time::now();//更新记录无人机姿态（如位置）的时间
    std_msgs::Float64 pos_errorx[3];//记录x轴误差,pos_error[0]表示这次误差，以此类推
    std_msgs::Float64 pos_errory[3];//记录y轴误差,pos_error[0]表示这次误差，以此类推
    std_msgs::Float64 pos_errorz[3];//记录z轴误差,pos_error[0]表示这次误差，以此类推
    for (int i = 0; i < 3; ++i) //初始化误差为10000米
    {
        pos_errorx[i].data = 10000;
        pos_errory[i].data = 10000;
        pos_errorz[i].data = 10000;
    }

    mavros_msgs::PositionTarget target_pose;//实例化一个mavros_msgs::PositionTarget类型的对象，并对其赋值，最后将其发布出去
    // 设置目标位置的相关参数
    target_pose.header.frame_id = "base_footprint";//设置坐标系的标识符
    target_pose.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;// 使用局部NED坐标系
    // 设置掩码，忽略加速度和偏航角速率，仅控制位置、速度和偏航角
    target_pose.type_mask = mavros_msgs::PositionTarget::IGNORE_AFX | mavros_msgs::PositionTarget::IGNORE_AFY | 
                                mavros_msgs::PositionTarget::IGNORE_AFZ | 
                                mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
    // 目标位置 (x, y, z)
    target_pose.position.x = 0.0;
    target_pose.position.y = 0.0;
    target_pose.position.z = 3.0;  // 飞到3米高度
    // 设置偏航角
    target_pose.yaw = 0.0;  // 0度偏航角

    //GPS部分
    // double target_lat = 47.397742;//初始lat：47.397742，lon：8.545593,alt:535.365917
    // double target_lon = 8.545593;
    // double target_alt = 535.365917;
    double target_lat[4]={0};//初始lat：47.397742，lon：8.545593,alt:535.365917
    double target_lon[4]={0};//定义变量来存储目标GPS信息，初始为0
    double target_alt[4]={0};
    // target_lat[0] = flydatas.gps[0].latitude;
    // target_lon[0] = flydatas.gps[0].longitude;//记录GPS初始位置
    // target_alt[0] = flydatas.gps[0].altitude;
    int8_t target_gps_order=0;
    // 记录计算得到的无人机从当前位置到目标点的END距离
    double dx, dy, dz;

    target_pose.position.x = local_pos.pose.position.x;
    target_pose.position.y = local_pos.pose.position.y;
    target_pose.position.z = local_pos.pose.position.z;     //无人机高度提高2米

    // target_pose.position.x = local_pos.pose.position.x - 10;
    // target_pose.position.y = local_pos.pose.position.y;
    // target_pose.position.z = local_pos.pose.position.z;     //无人机高度提高2米


    //创建修改参数服务请求和响应对象
    mavros_msgs::ParamSet param_srv;

    //设置无人机的相关参数
    /*
        MPC_XY_VEL_MAX    //水平面最大速度限制（XY方向），适用于 Offboard 等自动控制模式，单位 m/s
        MPC_Z_VEL_MAX_UP  //向上的最大垂直速度，单位 m/s
        MPC_Z_VEL_MAX_DN  //向下的最大垂直速度，单位 m/s
        MPC_TKO_SPEED     //无人机的起飞速度，单位 m/s
        MPC_LAND_SPEED    //无人机的降落速度，单位 m/s
        MPC_ACC_HOR_MAX   //水平最大加速度，单位 m/s²
        MPC_ACC_UP_MAX    //垂直上升最大加速度，单位 m/s²
        MPC_ACC_DOWN_MAX  //垂直下降最大加速度，单位 m/s²
        MPC_JERK_AUTO    //自动模式下的最大水平加速度变化率（即 jerk），单位 m/s³。影响飞行路径的平滑度
        MPC_JERK_MAN     //手动模式下的最大加速度变化率，单位 m/s³
        MPC_XY_VEL_ALL   //无人机在所有模式下的最大水平速度，单位为米/秒（m/s）
    */
    param_srv.request.param_id = "MPC_XY_VEL_MAX";//OFFBOARD模式下水平面最大速度限制（XY方向）
    param_srv.request.value.real = 15;  // 设置为 1.5 m/s
    // 调用服务
    if (param_set_client.call(param_srv)) {
        ROS_INFO("Parameter set: %s to %f", param_srv.request.param_id.c_str(), param_srv.request.value.real);
    } else {
        ROS_ERROR("Failed to call service param/set");
    }


    flydatas.doit = false;
    pub_flydatas.header.frame_id = "give you data";//发布的数据说明
    pub_flydatas.header.seq = 0;//初始化计数为0


    //在进入Offboard模式之前，必须已经启动了local_pos_pub数据流，否则模式切换将被拒绝。
    //这里的10可以被设置为任意数
    for(int i = 10; ros::ok() && i > 0; --i){
        mavros_msgs::PositionTarget test_pose;
        test_pose.position.x = local_pos.pose.position.x;
        test_pose.position.y = local_pos.pose.position.y;
        test_pose.position.z = local_pos.pose.position.z;
        local_pos_pub.publish(test_pose);
        ros::spinOnce();
        rate10.sleep();
    }

    if( current_state.mode != my_px4_mode.mode)//首先判断当前模式是否为offboard模式，如果不是，则进入if语句内部
    {
        offb_set_mode.request.custom_mode = "OFFBOARD";
        if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)//客户端set_mode_client向服务端offb_set_mode发起请求call，然后服务端回应response将模式返回，这就打开了my_px4_mode.data模式
        {
            ROS_INFO("%s enabled", my_px4_mode.mode.c_str());//打开Offboard模式后在终端打印信息
        }
        last_request_time = ros::Time::now();//更新时间
    }
    else ROS_INFO("%s:Drones used to be in OFFBOARD mode",file_name.c_str());
    if( !current_state.armed)//判断当前状态是否解锁，如果没有解锁，则进入if语句内部
    {
        //客户端arming_client向服务端arm_cmd发起请求call，然后服务端回应response成功解锁，则解锁成功
        if( arming_client.call(arm_cmd) && arm_cmd.response.success)
        {
            //解锁后在终端打印信息
            ROS_INFO("Vehicle armed");
            //更新时间
            last_request_time = ros::Time::now();
        }
        last_request_time = ros::Time::now();//更新时间
    }
    else ROS_INFO("%s:The drone was once unlocked",file_name.c_str());

    geometry_msgs::TwistStamped velocity_cmd;
    // 初始化速度指令 (单位：m/s)
    velocity_cmd.twist.linear.x = 0.0;  // 前向速度
    velocity_cmd.twist.linear.y = 0.0;  // 侧向速度
    velocity_cmd.twist.linear.z = 0.0;  // 垂直速度

    while (ros::ok()){
        //任务循环
        while(ros::ok() && flydatas.doit){
            ros::spinOnce();//处理一遍回调函数（有新消息时才执行，不然跳过回调函数）
            if(flydatas.doit && flydatas.task == 1.0){//任务为GPS巡航模式
                if( current_state.mode != my_px4_mode.mode)//首先判断当前模式是否为offboard模式，如果不是，则进入if语句内部
                {
                    //在进入Offboard模式之前，必须已经启动了local_pos_pub数据流，否则模式切换将被拒绝。
                    //这里的10可以被设置为任意数
                    for(int i = 10; ros::ok() && i > 0; --i){
                        mavros_msgs::PositionTarget test_pose;
                        test_pose.position.x = local_pos.pose.position.x;
                        test_pose.position.y = local_pos.pose.position.y;
                        test_pose.position.z = local_pos.pose.position.z;
                        local_pos_pub.publish(test_pose);
                        ros::spinOnce();
                        rate10.sleep();
                    }
                    offb_set_mode.request.custom_mode = "OFFBOARD";
                    if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)//客户端set_mode_client向服务端offb_set_mode发起请求call，然后服务端回应response将模式返回，这就打开了my_px4_mode.data模式
                    {
                        ROS_INFO("%s enabled", my_px4_mode.mode.c_str());//打开Offboard模式后在终端打印信息
                    }
                }
                if( !current_state.armed)//判断当前状态是否解锁，如果没有解锁，则进入if语句内部
                {
                    //客户端arming_client向服务端arm_cmd发起请求call，然后服务端回应response成功解锁，则解锁成功
                    if( arming_client.call(arm_cmd) && arm_cmd.response.success){
                        //解锁后在终端打印信息
                        ROS_INFO("Vehicle armed");
                        //更新时间
                        last_request_time = ros::Time::now();
                    }
                }
                // 更新消息的时间戳
                target_pose.header.stamp = ros::Time::now();
                //发布位置信息，所以综上飞机只有先打开offboard模式然后解锁才能飞起来
                local_pos_pub.publish(target_pose); 
                // // 发布速度指令
                // velocity_pub.publish(velocity_cmd);

                if(ros::Time::now() - last_local_pos_time > ros::Duration(0.8))//每隔一段时间记录一下与目标位置的差值，若连续3次在误差内，则认为已经到达目标位置
                {
                    pos_errorx[2].data = pos_errorx[1].data;//刷新误差
                    pos_errory[2].data = pos_errory[1].data;
                    pos_errorz[2].data = pos_errorz[1].data;

                    pos_errorx[1].data = pos_errorx[0].data;
                    pos_errory[1].data = pos_errory[0].data;
                    pos_errorz[1].data = pos_errorz[0].data;

                    pos_errorx[0].data = local_pos.pose.position.x - target_pose.position.x;
                    pos_errory[0].data = local_pos.pose.position.y - target_pose.position.y;
                    pos_errorz[0].data = local_pos.pose.position.z - target_pose.position.z;
                    
                    ROS_INFO("local_pos.x,y,z:%f,%f,%f",local_pos.pose.position.x,local_pos.pose.position.y,local_pos.pose.position.z);
                    ROS_WARN("pos_errorx[0].data:%f,pos_errory[0].data:%f,pos_errorz[0].data:%f",pos_errorx[0].data,pos_errory[0].data,pos_errorz[0].data);

                    ROS_INFO("now:lat:%f,lon:%f,alt:%f",global_pos.latitude,global_pos.longitude,global_pos.altitude);

                    bool all_in_range = true;
                    for (int i = 0; i < 3; ++i) {
                        if (!(pos_errorx[i].data > -0.4 && pos_errorx[i].data < 0.4) ||
                            !(pos_errory[i].data > -0.4 && pos_errory[i].data < 0.4) ||
                            !(pos_errorz[i].data > -0.4 && pos_errorz[i].data < 0.4)) {
                            all_in_range = false;
                            break; // 只要有一个不满足条件，就可以退出循环
                        }
                    }
                    if (all_in_range) {
                        // 所有数据都在 -0.4 到 0.4 之间，执行相应的逻辑
                        for (int i = 0; i < 3; ++i) //初始化误差为10000米
                        {
                            pos_errorx[i].data = 10000;
                            pos_errory[i].data = 10000;
                            pos_errorz[i].data = 10000;
                        }
                        ROS_INFO("the position is ok");
                        // 在程序结束前切换到保持模式
                        hold_mode.request.custom_mode = "AUTO.LOITER";  //AUTO.LOITER:保持当前位置模式
                        if (set_mode_client.call(hold_mode) && hold_mode.response.mode_sent) {
                            ROS_INFO("Switched to LOITER mode. The drone will maintain its position.");
                        }
                        target_gps_order ++;
                        if(target_gps_order>3){//超过最大任务点数
                            flydatas.task = 0.0;
                            flydatas.doit = false;
                            ROS_WARN("If the maximum number of mission points is exceeded, the mission is completed");
                            ROS_WARN("Toggle to the Non-Task loop,flydatas.doit = false,flydatas.task = 0.0");
                            break;
                        }
                        else{
                            if(target_lat[target_gps_order]!=0&&target_lon[target_gps_order]!=0&&target_alt[target_gps_order]!=0){//有任务点
                                // target_lat[0] = flydatas.gps[0].latitude;//初始lat：47.397742，lon：8.545593,alt:535.365917
                                // target_lon[0] = flydatas.gps[0].longitude;
                                // target_alt[0] = flydatas.gps[0].altitude;

                                ROS_INFO("%s:wait for global_gps",file_name.c_str());
                                while(ros::ok()&&global_pos.status.status<0){//等待GPS定位
                                    ros::spinOnce();
                                    rate10.sleep();
                                }
                                globalToLocalEND(global_pos.latitude, global_pos.longitude, target_lat[target_gps_order], target_lon[target_gps_order], dx, dy);
                                dz = target_alt[target_gps_order] - global_pos.altitude;
                                if(dz>=30)dz = 30;      //高度限制
                                if(dz<=-50)dz = -50;    //高度限制
                                if(dx>=150)dx = 100;    //x轴限制
                                if(dx<=-150)dx=-100;    //x轴限制
                                if(dy>=150)dy = 100;    //y轴限制
                                if(dy<=-150)dy = -100;  //y轴限制
                                ROS_INFO("lat:%f,lon:%f,alt:%f",global_pos.latitude,global_pos.longitude,global_pos.altitude);
                                ROS_INFO("target_lat[%d]:%f,target_lon[%d]:%f,target_alt[%d]:%f。",target_gps_order,target_lat[target_gps_order],target_gps_order,target_lon[target_gps_order],target_gps_order,target_alt[target_gps_order]);
                                ROS_INFO("dx:%f,dy:%f,dz:%f",dx,dy,dz);
                                target_pose.position.x = local_pos.pose.position.x + dx;
                                target_pose.position.y = local_pos.pose.position.y + dy;
                                target_pose.position.z = local_pos.pose.position.z + dz;
                            }else{
                                flydatas.task = 0.0;
                                flydatas.doit = false;
                                ROS_WARN("Toggle to the Non-Task loop,flydatas.doit = false,flydatas.task = 0.0");
                                break;
                            }
                        }
                    }
                    last_local_pos_time = ros::Time::now();//更新记录无人机姿态（如位置）的时间
                }
            }
            else if(flydatas.doit && flydatas.task == 2.0){//任务为GPS定点降落
                if( current_state.mode != my_px4_mode.mode)//首先判断当前模式是否为offboard模式，如果不是，则进入if语句内部
                {
                    //在进入Offboard模式之前，必须已经启动了local_pos_pub数据流，否则模式切换将被拒绝。
                    //这里的10可以被设置为任意数
                    for(int i = 10; ros::ok() && i > 0; --i){
                        mavros_msgs::PositionTarget test_pose;
                        test_pose.position.x = local_pos.pose.position.x;
                        test_pose.position.y = local_pos.pose.position.y;
                        test_pose.position.z = local_pos.pose.position.z;
                        local_pos_pub.publish(test_pose);
                        ros::spinOnce();
                        rate10.sleep();
                    }
                    offb_set_mode.request.custom_mode = "OFFBOARD";
                    if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)//客户端set_mode_client向服务端offb_set_mode发起请求call，然后服务端回应response将模式返回，这就打开了my_px4_mode.data模式
                    {
                        ROS_INFO("%s enabled", my_px4_mode.mode.c_str());//打开Offboard模式后在终端打印信息
                    }
                }
                if( !current_state.armed)//判断当前状态是否解锁，如果没有解锁，则进入if语句内部
                {
                    //客户端arming_client向服务端arm_cmd发起请求call，然后服务端回应response成功解锁，则解锁成功
                    if( arming_client.call(arm_cmd) && arm_cmd.response.success){
                        //解锁后在终端打印信息
                        ROS_INFO("Vehicle armed");
                        //更新时间
                        last_request_time = ros::Time::now();
                    }
                }
                // 更新消息的时间戳
                target_pose.header.stamp = ros::Time::now();
                //发布位置信息，所以综上飞机只有先打开offboard模式然后解锁才能飞起来
                local_pos_pub.publish(target_pose);
                if(ros::Time::now() - last_local_pos_time > ros::Duration(0.8))//每隔一段时间记录一下与目标位置的差值，若连续3次在误差内，则认为已经到达目标位置
                {
                    pos_errorx[2].data = pos_errorx[1].data;//刷新误差
                    pos_errory[2].data = pos_errory[1].data;
                    pos_errorz[2].data = pos_errorz[1].data;

                    pos_errorx[1].data = pos_errorx[0].data;
                    pos_errory[1].data = pos_errory[0].data;
                    pos_errorz[1].data = pos_errorz[0].data;

                    pos_errorx[0].data = local_pos.pose.position.x - target_pose.position.x;
                    pos_errory[0].data = local_pos.pose.position.y - target_pose.position.y;
                    pos_errorz[0].data = local_pos.pose.position.z - target_pose.position.z;
                    
                    ROS_INFO("local_pos.x,y,z:%f,%f,%f",local_pos.pose.position.x,local_pos.pose.position.y,local_pos.pose.position.z);
                    ROS_WARN("pos_errorx[0].data:%f,pos_errory[0].data:%f,pos_errorz[0].data:%f",pos_errorx[0].data,pos_errory[0].data,pos_errorz[0].data);

                    ROS_INFO("now:lat:%f,lon:%f,alt:%f",global_pos.latitude,global_pos.longitude,global_pos.altitude);
                    
                    std_msgs::Float64 allowable_error;        //设置一个允许误差，默认为0.4m
                    allowable_error.data = 0.4;
                    if(target_gps_order == 0 || target_gps_order == 1 || target_gps_order == 2){
                        allowable_error.data = 0.4;      //上升阶段、返回阶段、下降阶段允许误差为0.4m
                    }
                    else{
                        allowable_error.data = 0.05;      //降落阶段允许误差为0.05m
                    }
                    
                    bool all_in_range = true;
                    for (int i = 0; i < 3; ++i) {
                        if (!(pos_errorx[i].data > -allowable_error.data && pos_errorx[i].data < allowable_error.data) ||
                            !(pos_errory[i].data > -allowable_error.data && pos_errory[i].data < allowable_error.data) ||
                            !(pos_errorz[i].data > -0.4 && pos_errorz[i].data < 0.4)) {
                            all_in_range = false;
                            break; // 只要有一个不满足条件，就可以退出循环
                        }
                    }
                    if (all_in_range) {
                        for (int i = 0; i < 3; ++i) //初始化误差为10000米
                        {
                            pos_errorx[i].data = 10000;
                            pos_errory[i].data = 10000;
                            pos_errorz[i].data = 10000;
                        }
                        // 所有数据都在 -allowable_error 到 allowable_error 之间，执行相应的逻辑
                        ROS_INFO("the position is ok");
                        // 在程序结束前切换到保持模式
                        hold_mode.request.custom_mode = "AUTO.LOITER";  //AUTO.LOITER:保持当前位置模式
                        if (set_mode_client.call(hold_mode) && hold_mode.response.mode_sent) {
                            ROS_INFO("Switched to LOITER mode. The drone will maintain its position.");
                        }

                        target_gps_order ++;
                        ROS_INFO("target_gps_order is %d",target_gps_order);
                        if(target_gps_order == 1 && target_lat[0]!=0&&target_lon[0]!=0&&target_alt[0]!=0)
                        {//返回阶段
                            ROS_INFO("%s:wait for global_gps",file_name.c_str());
                            while(ros::ok()&&global_pos.status.status<0){//等待GPS定位
                                ros::spinOnce();
                                rate10.sleep();
                            }
                            globalToLocalEND(global_pos.latitude, global_pos.longitude, target_lat[0], target_lon[0], dx, dy);
                            dz = 0;         //返回阶段保持高度不变
                            if(dz>=30)dz = 30;      //高度限制
                            if(dz<=-50)dz = -50;    //高度限制
                            if(dx>=150)dx = 100;    //x轴限制
                            if(dx<=-150)dx=-100;    //x轴限制
                            if(dy>=150)dy = 100;    //y轴限制
                            if(dy<=-150)dy = -100;  //y轴限制
                            ROS_INFO("lat:%f,lon:%f,alt:%f",global_pos.latitude,global_pos.longitude,global_pos.altitude);
                            ROS_INFO("target_lat[%d]:%f,target_lon[%d]:%f。",0,target_lat[0],0,target_lon[0]);
                            ROS_INFO("dx:%f,dy:%f,dz:%f",dx,dy,dz);
                            target_pose.position.x = local_pos.pose.position.x + dx;
                            target_pose.position.y = local_pos.pose.position.y + dy;
                            target_pose.position.z = local_pos.pose.position.z + dz;
                        }
                        else if (target_gps_order == 2 && target_lat[0]!=0&&target_lon[0]!=0&&target_alt[0]!=0)
                        {//下降阶段
                            ROS_INFO("%s:wait for global_gps",file_name.c_str());
                            while(ros::ok()&&global_pos.status.status<0){//等待GPS定位
                                ros::spinOnce();
                                rate10.sleep();
                            }
                            dx = 0;
                            dy = 0;
                            dz = 1.2 - local_pos.pose.position.z;        //调整高度为降落点上方1.2m
                            dz = target_alt[0] - global_pos.altitude + 1.2;        //下降阶段下降高度到距离目标点1.2m
                            if(dz>=30)dz = 30;      //高度限制
                            if(dz<=-50)dz = -50;    //高度限制
                            if(dx>=150)dx = 150;    //x轴限制
                            if(dx<=-150)dx=-150;    //x轴限制
                            if(dy>=150)dy = 150;    //y轴限制
                            if(dy<=-150)dy = -150;  //y轴限制
                            ROS_INFO("lat:%f,lon:%f,alt:%f",global_pos.latitude,global_pos.longitude,global_pos.altitude);
                            ROS_INFO("target_lat[%d]:%f,target_lon[%d]:%f,target_alt[%d]:%f。",0,target_lat[0],0,target_lon[0],0,target_alt[0]+1.2);
                            ROS_INFO("dx:%f,dy:%f,dz:%f",dx,dy,dz);
                            target_pose.position.x = local_pos.pose.position.x + dx;
                            target_pose.position.y = local_pos.pose.position.y + dy;
                            target_pose.position.z = local_pos.pose.position.z + dz;
                        }
                        else if(target_lat[0]!=0&&target_lon[0]!=0&&target_alt[0]!=0)
                        {//平移到目标上方降落阶段(除了target_gps_order的值为1和2，理论上到不了目标位置的话，不介入的话，这里会一直执行下去)
                            ROS_INFO("jieduan3");
                            ROS_INFO("%s:wait for global_gps",file_name.c_str());
                            while(ros::ok()&&global_pos.status.status<0){//等待GPS定位
                                ros::spinOnce();
                                rate10.sleep();
                            }
                            if(global_pos.latitude - target_lat[0]<=0.000001 
                            && global_pos.latitude - target_lat[0]>=-0.000001
                            && global_pos.longitude - target_lon[0]<=0.000001
                            && global_pos.longitude - target_lon[0]>=-0.000001){//无人机经纬度在允许的范围内
                                // 切换模式到降落模式，让无人机降落
                                hold_mode.request.custom_mode = "AUTO.LAND";  //AUTO.LAND:自动降落模式
                                if (set_mode_client.call(hold_mode) && hold_mode.response.mode_sent) {
                                    ROS_INFO("Switched to AUTO.LAND mode. The drone will AUTO.LAND.");
                                    ROS_INFO("The drone is adjusted and waits for the drone to land");
                                    // 等待无人机完成降落
                                    while (ros::ok()) {
                                        ros::spinOnce();
                                        // 检查飞行模式是否为 "AUTO.LAND" 并且无人机是否解除锁定
                                        hold_mode.request.custom_mode = "AUTO.LAND";  //AUTO.LOITER:保持当前位置模式
                                        if(current_state.mode != "AUTO.LAND")
                                        {
                                            if (set_mode_client.call(hold_mode) && hold_mode.response.mode_sent) {
                                                ROS_INFO("Switched to AUTO.LAND mode. The drone will AUTO.LAND.");
                                            }
                                            else{
                                                ROS_INFO("%s:Failed to switch to AUTO.LOITER",file_name.c_str());
                                            }
                                        }
                                        // if(global_pos.altitude - target_alt[0]>= -0.2 && global_pos.altitude - target_alt[0]<= 0.2){
                                        if(local_pos.pose.position.z>= -0.2 && local_pos.pose.position.z<= 0.2){
                                            //到达降落点
                                            std::this_thread::sleep_for(std::chrono::seconds(5));//等待5s，等待无人机降落成功
                                            ROS_WARN("The drone landed successfully.");
                                            flydatas.task = 0.0;
                                            flydatas.doit = false;
                                            ROS_WARN("Toggle to the Non-Task loop,flydatas.doit = false,flydatas.task = 0.0");
                                            break;
                                        }
                                        rate5.sleep();
                                    }
                                    break;
                                }
                            }
                            else{//无人机经纬度不在位置允许范围内
                                ROS_INFO("The latitude and longitude of the drone are not within the allowable range of the location");
                                globalToLocalEND(global_pos.latitude, global_pos.longitude, target_lat[0], target_lon[0], dx, dy);//计算误差
                                dz = 0;     //降落阶段调整位置时保持高度不变
                                if(dz>=30)dz = 30;      //高度限制
                                if(dz<=-50)dz = -50;    //高度限制
                                if(dx>=150)dx = 100;    //x轴限制
                                if(dx<=-150)dx=-100;    //x轴限制
                                if(dy>=150)dy = 100;    //y轴限制
                                if(dy<=-150)dy = -100;  //y轴限制
                                ROS_INFO("lat:%f,lon:%f,alt:%f",global_pos.latitude,global_pos.longitude,global_pos.altitude);
                                ROS_INFO("target_lat[%d]:%f,target_lon[%d]:%f。",0,target_lat[0],0,target_lon[0]);
                                ROS_INFO("dx:%f,dy:%f,dz:%f",dx,dy,dz);
                                target_pose.position.x = local_pos.pose.position.x + dx;
                                target_pose.position.y = local_pos.pose.position.y + dy;
                                target_pose.position.z = local_pos.pose.position.z + dz;
                                ROS_INFO("Number of landing adjustments:%d",target_gps_order - 2);
                                if(target_gps_order - 2 >= 10){//最多调整位置10次，超过这个次数就认为是无法调整回来了
                                    flydatas.task = 0.0;
                                    flydatas.doit = false;
                                    ROS_WARN("Toggle to the Non-Task loop,flydatas.doit = false,flydatas.task = 0.0");
                                    ROS_WARN("The drone failed to adjust and exited the mission");
                                    break;
                                }
                            }
                        }
                    }
                    last_local_pos_time = ros::Time::now();//更新记录无人机姿态（如位置）的时间
                }
            }
            else if(flydatas.doit && flydatas.task == 3.0){//任务为无人机速度控制
                if( current_state.mode != my_px4_mode.mode)//首先判断当前模式是否为offboard模式，如果不是，则进入if语句内部
                {
                    //在进入Offboard模式之前，必须已经启动了local_pos_pub数据流，否则模式切换将被拒绝。
                    //这里的10可以被设置为任意数
                    for(int i = 10; ros::ok() && i > 0; --i){
                        mavros_msgs::PositionTarget test_pose;
                        test_pose.position.x = local_pos.pose.position.x;
                        test_pose.position.y = local_pos.pose.position.y;
                        test_pose.position.z = local_pos.pose.position.z;
                        local_pos_pub.publish(test_pose);
                        ros::spinOnce();
                        rate10.sleep();
                    }
                    offb_set_mode.request.custom_mode = "OFFBOARD";
                    if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)//客户端set_mode_client向服务端offb_set_mode发起请求call，然后服务端回应response将模式返回，这就打开了my_px4_mode.data模式
                    {
                        ROS_INFO("%s enabled", my_px4_mode.mode.c_str());//打开Offboard模式后在终端打印信息
                    }
                }
                if( !current_state.armed)//判断当前状态是否解锁，如果没有解锁，则进入if语句内部
                {
                    //客户端arming_client向服务端arm_cmd发起请求call，然后服务端回应response成功解锁，则解锁成功
                    if( arming_client.call(arm_cmd) && arm_cmd.response.success){
                        //解锁后在终端打印信息
                        ROS_INFO("Vehicle armed");
                        //更新时间
                        last_request_time = ros::Time::now();
                    }
                }
                double lon = 0.0;   //定义变量来临时存储数据，lon来存储经度，lat来存储纬度，alt来存储海拔
                double lat = 0.0;
                double alt = 0.0;
                lon = flydatas.gps[0].longitude;
                lat = flydatas.gps[0].latitude;
                alt = flydatas.gps[0].altitude;
                if(lon == 0.0){  //任务是保持禁止
                    velocity_cmd.twist.linear.x = 0.0;  // 前向速度
                    velocity_cmd.twist.linear.y = 0.0;  // 侧向速度
                }
                else if (lon == 1.0)    //任务为速度向前
                {
                    velocity_cmd.twist.linear.x = lat;  // 赋值前向速度
                    velocity_cmd.twist.linear.y = 0.0;  // 赋值侧向速度
                }
                else if (lon == 2.0)    //任务为速度向后
                {
                    velocity_cmd.twist.linear.x = -lat;  // 赋值前向速度
                    velocity_cmd.twist.linear.y = 0.0;  // 赋值侧向速度
                }
                else if (lon == 3.0)    //任务为速度向左
                {
                    velocity_cmd.twist.linear.x = 0.0;  // 赋值前向速度
                    velocity_cmd.twist.linear.y = lat;  // 赋值侧向速度
                }
                else if (lon == 4.0)    //任务为速度向右
                {
                    velocity_cmd.twist.linear.x = 0.0;  // 赋值前向速度
                    velocity_cmd.twist.linear.y = -lat;  // 赋值侧向速度
                }
                else if (lon == 5.0)    //任务为速度向上（先暂停开发）
                {
                    target_pose.position.z = local_pos.pose.position.z + alt;
                    ROS_INFO("5.0target_pose.position.z is %f",target_pose.position.z);
                    velocity_cmd.twist.linear.z = 0.0;  // 赋值垂直速度
                }
                else if (lon == 6.0)    //任务为速度向下（先暂停开发）
                {
                    target_pose.position.z = local_pos.pose.position.z - alt;
                    ROS_INFO("6.0target_pose.position.z is %f",target_pose.position.z);
                    velocity_cmd.twist.linear.z = 0.0;  // 赋值垂直速度
                }
                double error = target_pose.position.z - local_pos.pose.position.z;
                velocity_cmd.twist.linear.z = error*10;
                // if (velocity_cmd.twist.linear.z<=-1){velocity_cmd.twist.linear.z = -1;}
                // else if (velocity_cmd.twist.linear.z>=1){velocity_cmd.twist.linear.z = 1;}
                if (velocity_cmd.twist.linear.z != 0)
                {
                    ROS_INFO("velocity_cmd.twist.linear.z is %f",velocity_cmd.twist.linear.z);
                }
                
                
                

                // 更新消息的时间戳
                velocity_cmd.header.frame_id = "speed_control";
                velocity_cmd.header.seq ++;
                velocity_cmd.header.stamp = ros::Time::now();
                // 发布速度指令
                velocity_pub.publish(velocity_cmd);
            }
            else if(!flydatas.doit){
                //控制无人机的相关变量恢复初始值
                target_pose.position.x = local_pos.pose.position.x;         //位置为当前位置
                target_pose.position.y = local_pos.pose.position.y;
                target_pose.position.z = local_pos.pose.position.z;

                velocity_cmd.twist.linear.x = 0;    //速度归零
                velocity_cmd.twist.linear.y = 0;
                velocity_cmd.twist.linear.z = 0;

                flydatas.task = 0;  //任务归零，表示无任务状态

                pub_flydatas.doit = true;//表示已经接收到gps数据，让发布者停止发送
                ROS_INFO("%s:wait 2 times.",file_name.c_str());
                for(int i = 0;i<2;i++){//发布消息2次，确保火焰那边能订阅到
                    //发布飞行数据给火焰处理者
                    pub_flydatas.header.seq++;
                    pub_flydatas.header.stamp = ros::Time::now();
                    ros::spinOnce();
                    flydatas_pub.publish(pub_flydatas);//发布数据给控制者（发布gps控制数据者，火焰那边的）
                    rate10.sleep();
                }
                pub_flydatas.doit = false;//恢复等待数据的状态
            }
            //发布飞行数据给火焰处理者
            pub_flydatas.header.seq++;
            pub_flydatas.header.stamp = ros::Time::now();
            flydatas_pub.publish(pub_flydatas);//发布数据给控制者（发布gps控制数据者，火焰那边的）
            rate10.sleep();
        }
        //非任务循环
        while(ros::ok && !flydatas.doit){
            ros::spinOnce();//刷新话题消息
            hold_mode.request.custom_mode = "AUTO.LOITER";  //AUTO.LOITER:保持当前位置模式
            if(current_state.mode != "AUTO.LOITER")
            {
                if (set_mode_client.call(hold_mode) && hold_mode.response.mode_sent) {
                    ROS_INFO("Switched to LOITER mode. The drone will maintain its position.");
                }
                else{
                    ROS_INFO("%s:Failed to switch to AUTO.LOITER",file_name.c_str());
                }
            }
            if(flydatas.doit && flydatas.task == 1.0){//有任务过来,任务是1.0,GPS巡航模式
                ROS_INFO("%s:Yes Tasks,task code is %.1f cruise",file_name.c_str(), flydatas.task);
                if(flydatas.gps[0].latitude!=0&&flydatas.gps[0].longitude!=0&&flydatas.gps[0].altitude!=0){//有GPS信号
                    // target_lat[0] = flydatas.gps[0].latitude;//初始lat：47.397742，lon：8.545593,alt:535.365917
                    // target_lon[0] = flydatas.gps[0].longitude;
                    // target_alt[0] = flydatas.gps[0].altitude;
                    for (size_t i = 0; i < flydatas.gps.size(); ++i) {
                        target_lat[i] = flydatas.gps[i].latitude;
                        target_lon[i] = flydatas.gps[i].longitude;
                        target_alt[i] = flydatas.gps[i].altitude;
                    }
                    pub_flydatas.doit = true;//表示已经接收到gps数据，让发布者停止发送
                    ROS_INFO("%s:wait 5 times.",file_name.c_str());
                    for(int i = 0;i<5;i++){//发布消息5次，确保火焰那边能订阅到
                        //发布飞行数据给火焰处理者
                        pub_flydatas.header.seq++;
                        pub_flydatas.header.stamp = ros::Time::now();
                        ros::spinOnce();
                        flydatas_pub.publish(pub_flydatas);//发布数据给控制者（发布gps控制数据者，火焰那边的）
                        rate10.sleep();
                    }
                    pub_flydatas.doit = false;//恢复等待数据的状态
                    
                    ROS_INFO("%s:wait for global_gps",file_name.c_str());
                    while(ros::ok()&&global_pos.status.status<0){//等待GPS定位
                        ros::spinOnce();
                        rate10.sleep();
                    }
                    globalToLocalEND(global_pos.latitude, global_pos.longitude, target_lat[0], target_lon[0], dx, dy);
                    dz = target_alt[0] - global_pos.altitude;
                    if(dz>=30)dz = 30;      //高度限制
                    if(dz<=-50)dz = -50;    //高度限制
                    if(dx>=150)dx = 100;    //x轴限制
                    if(dx<=-150)dx=-100;    //x轴限制
                    if(dy>=150)dy = 100;    //y轴限制
                    if(dy<=-150)dy = -100;  //y轴限制
                    ROS_INFO("lat:%f,lon:%f,alt:%f",global_pos.latitude,global_pos.longitude,global_pos.altitude);
                    ROS_INFO("target_lat[0]:%f,target_lon[0]:%f,target_alt[0]:%f。",target_lat[0],target_lon[0],target_alt[0]);
                    ROS_INFO("dx:%f,dy:%f,dz:%f",dx,dy,dz);
                    target_gps_order = 0;//初始化GPS任务为第一个定位点
                    target_pose.position.x = local_pos.pose.position.x + dx;
                    target_pose.position.y = local_pos.pose.position.y + dy;
                    target_pose.position.z = local_pos.pose.position.z + dz;
                }
                else{
                    flydatas.task = 0.0;
                    flydatas.doit = false;
                    ROS_WARN("Toggle to the Non-Task loop,flydatas.doit = false,flydatas.task = 0.0");
                    ROS_WARN("%s:The GPS value is incorrect, and the task is refused",file_name.c_str());
                }
            }
            else if(flydatas.doit && flydatas.task == 2.0){//有任务过来，任务是GPS定点降落（可以降落在充电站）
                ROS_INFO("%s:Yes Tasks,task code is %.1f GPS",file_name.c_str(), flydatas.task);
                if(flydatas.gps[0].latitude!=0&&flydatas.gps[0].longitude!=0&&flydatas.gps[0].altitude!=0){//有GPS降落点
                    // target_lat[0] = flydatas.gps[0].latitude;//初始lat：47.397742，lon：8.545593,alt:535.365917
                    // target_lon[0] = flydatas.gps[0].longitude;
                    // target_alt[0] = flydatas.gps[0].altitude;
                    for (size_t i = 0; i < flydatas.gps.size(); ++i) {
                        target_lat[i] = flydatas.gps[i].latitude;
                        target_lon[i] = flydatas.gps[i].longitude;
                        target_alt[i] = flydatas.gps[i].altitude;
                    }
                    pub_flydatas.doit = true;//表示已经接收到gps数据，让发布者停止发送
                    ROS_INFO("%s:wait 5 times.",file_name.c_str());
                    for(int i = 0;i<5;i++){//发布消息5次，确保火焰那边能订阅到
                        //发布飞行数据给火焰处理者
                        pub_flydatas.header.seq++;
                        pub_flydatas.header.stamp = ros::Time::now();
                        ros::spinOnce();
                        flydatas_pub.publish(pub_flydatas);//发布数据给控制者（发布gps控制数据者，火焰那边的）
                        rate10.sleep();
                    }
                    pub_flydatas.doit = false;//恢复等待数据的状态
                    
                    ROS_INFO("%s:The height of the drone is raised by 3m",file_name.c_str());
                    target_gps_order = 0;//初始化GPS任务为第一个定位点
                    target_pose.position.x = local_pos.pose.position.x;
                    target_pose.position.y = local_pos.pose.position.y;
                    target_pose.position.z = local_pos.pose.position.z + 3;
                }
                else{
                    flydatas.task = 0.0;
                    flydatas.doit = false;
                    ROS_WARN("Toggle to the Non-Task loop,flydatas.doit = false,flydatas.task = 0.0");
                    ROS_WARN("%s:The GPS value is incorrect, and the task is refused",file_name.c_str());
                }
            }
            else if(flydatas.doit && flydatas.task == 3.0){//有任务过来，任务是速度控制
                ROS_INFO("%s:Yes Tasks,task code is %.1f speed",file_name.c_str(), flydatas.task);
                double lon = 0.0;       //定义lon来存储经度，lat来存储纬度，alt来存储海拔
                double lat = 0.0;
                double alt = 0.0;
                lon = flydatas.gps[0].longitude;
                lat = flydatas.gps[0].latitude;
                alt = flydatas.gps[0].altitude;
                if(lon == 0.0){  //任务是保持禁止
                    velocity_cmd.twist.linear.x = 0.0;  // 前向速度
                    velocity_cmd.twist.linear.y = 0.0;  // 侧向速度
                    velocity_cmd.twist.linear.z = 0.0;  // 垂直速度
                }
                else if (lon == 1.0)    //任务为速度向前
                {
                    velocity_cmd.twist.linear.x = lat;  // 赋值前向速度
                    velocity_cmd.twist.linear.y = 0.0;  // 赋值侧向速度
                    velocity_cmd.twist.linear.z = 0.0;  // 赋值垂直速度
                }
                else if (lon == 2.0)    //任务为速度向后
                {
                    velocity_cmd.twist.linear.x = -lat;  // 赋值前向速度
                    velocity_cmd.twist.linear.y = 0.0;  // 赋值侧向速度
                    velocity_cmd.twist.linear.z = 0.0;  // 赋值垂直速度
                }
                else if (lon == 3.0)    //任务为速度向左
                {
                    velocity_cmd.twist.linear.x = 0.0;  // 赋值前向速度
                    velocity_cmd.twist.linear.y = lat;  // 赋值侧向速度
                    velocity_cmd.twist.linear.z = 0.0;  // 赋值垂直速度
                }
                else if (lon == 4.0)    //任务为速度向右
                {
                    velocity_cmd.twist.linear.x = 0.0;  // 赋值前向速度
                    velocity_cmd.twist.linear.y = -lat;  // 赋值侧向速度
                    velocity_cmd.twist.linear.z = 0.0;  // 赋值垂直速度
                }
                else if (lon == 5.0)    //任务为向上移动指定距离（先暂停开发）
                {
                    target_pose.position.z = local_pos.pose.position.z + alt;
                    velocity_cmd.twist.linear.z = 0.0;  // 赋值垂直速度
                }
                else if (lon == 6.0)    //任务为向下移动指定距离（先暂停开发）
                {
                    target_pose.position.z = local_pos.pose.position.z - alt;
                    velocity_cmd.twist.linear.z = 0.0;  // 赋值垂直速度
                }
                pub_flydatas.doit = true;//表示已经接收到gps数据，让发布者停止发送
                ROS_INFO("%s:wait 2 times.",file_name.c_str());
                for(int i = 0;i<2;i++){//发布消息2次，确保火焰那边能订阅到
                    //发布飞行数据给火焰处理者
                    pub_flydatas.header.seq++;
                    pub_flydatas.header.stamp = ros::Time::now();
                    ros::spinOnce();
                    flydatas_pub.publish(pub_flydatas);//发布数据给控制者（发布gps控制数据者，火焰那边的）
                    rate10.sleep();
                }
                pub_flydatas.doit = false;//恢复等待数据的状态


            }
            //发布飞行数据给火焰处理者
            pub_flydatas.header.seq++;
            pub_flydatas.header.stamp = ros::Time::now();
            flydatas_pub.publish(pub_flydatas);//发布数据给控制者（发布gps控制数据者，火焰那边的）
            //rate5标志5Hz
            rate5.sleep();
        }
        ros::spinOnce();//处理一遍回调函数（有新消息时才执行，不然跳过回调函数）
        //发布飞行数据给火焰处理者
        pub_flydatas.header.seq++;
        pub_flydatas.header.stamp = ros::Time::now();
        flydatas_pub.publish(pub_flydatas);//发布数据给控制者（发布gps控制数据者，火焰那边的）
        //rate10表示10Hz
        rate10.sleep();//放置在循环中，控制循环的执行速率（未必是主循环，可以设置多个rate对象控制不同的速度）
    }
    return 0;
}
